/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include "CommunicableSensor.h"
#include "AngularVelocimeter.h"
#include "LinearVelocimeter.h"
#include "TactileSensor.h"
#include "TactileMatrix.h"
#include "Camera.h"
#include "PositionSensor.h"
#include "CorbaImplementations.h"

template<>
void CommunicableSensor<TactileMatrix>::copySensor(VSensorOut *vSensor) {
	TactileMatrix *tactile = dynamic_cast<TactileMatrix*>(this);
	VTactileMatrixOut *vTactile = dynamic_cast<VTactileMatrixOut*>(vSensor);
	std::vector<TactileSensor*>& sensors = tactile->getSensors();
	for(unsigned i = 0; i < sensors.size(); i++) {
		TactileSensor *sensor = sensors[i];
		(*vTactile->sensors())[i]->activation(sensor->getActivation());
	}
}

template<>
void CommunicableSensor<TactileSensor>::copySensor(VSensorOut *vSensor) {
	TactileSensor *tactile = dynamic_cast<TactileSensor*>(this);
	VTactileSensorOut *vTactile = dynamic_cast<VTactileSensorOut*>(vSensor);
	vTactile->activation(tactile->getActivation());
}

template<>
void CommunicableSensor<AngularVelocimeter>::copySensor(VSensorOut *vSensor) {
	AngularVelocimeter *velocimeter = dynamic_cast<AngularVelocimeter*>(this);
	Vector3D &v = velocimeter->getVelocity();
	VAngularVelocimeterOut *vAngularVelocimeter = dynamic_cast<VAngularVelocimeterOut*>(vSensor);
	vAngularVelocimeter->x(v.x);
	vAngularVelocimeter->y(v.y);
	vAngularVelocimeter->z(v.z);
}

template<>
void CommunicableSensor<LinearVelocimeter>::copySensor(VSensorOut *vSensor) {
	LinearVelocimeter *velocimeter = dynamic_cast<LinearVelocimeter*>(this);
	Vector3D &v = velocimeter->getVelocity();
	VLinearVelocimeterOut *vLinearVelocimeter = dynamic_cast<VLinearVelocimeterOut*>(vSensor);
	vLinearVelocimeter->x(v.x);
	vLinearVelocimeter->y(v.y);
	vLinearVelocimeter->z(v.z);
}

template<>
void CommunicableSensor<PositionSensor>::copySensor(VSensorOut *vSensor) {
	PositionSensor *sensor = dynamic_cast<PositionSensor*>(this);
	Vector3D pos = sensor->getParentEntity()->getPosition();
	VPositionSensorOut *vPositionSensor = dynamic_cast<VPositionSensorOut*>(vSensor);
	vPositionSensor->x(pos.x);
	vPositionSensor->y(pos.y);
	vPositionSensor->z(pos.z);
}
	
template<>
void CommunicableSensor<Camera>::copySensor(VSensorOut *vSensor) {
}

template<>
VSensorOut* CommunicableSensor<Camera>::activateSensor() {
	Camera *camera = dynamic_cast<Camera*>(this);
	int size = camera->getHorizontalResolution() * camera->getVerticalResolution() * camera->getDepth();
	VCameraOut* vCamera = new VCameraOut_i(camera->getHorizontalResolution(), camera->getVerticalResolution(), camera->getDepth(), new VImageOut(size, size, (CORBA::Octet*) camera->getBuffer()));
	return vCamera;
}

template<>
VSensorOut* CommunicableSensor<TactileMatrix>::activateSensor() {
	TactileMatrix* tactile = dynamic_cast<TactileMatrix*>(this);
	VTactileMatrixOut *vTactile = new VTactileMatrixOut_i(new VTactileSensorsOut());
	std::vector<TactileSensor*>& sensors = tactile->getSensors();
	vTactile->sensors()->length(sensors.size());
	for(unsigned i = 0; i < sensors.size(); i++) {
		TactileSensor *sensor = sensors[i];
		(*vTactile->sensors())[i] = new VTactileSensorOut_i(sensor->getX(), sensor->getY(), sensor->getZ(), sensor->getActivation());
	}
	return vTactile;
}

template<>
VSensorOut* CommunicableSensor<TactileSensor>::activateSensor() {
	TactileSensor* tactile = dynamic_cast<TactileSensor*>(this);
	VTactileSensorOut *vTactile = new VTactileSensorOut_i(tactile->getX(), tactile->getY(), tactile->getZ(), tactile->getActivation());
	return vTactile;
}

template<>
VSensorOut* CommunicableSensor<AngularVelocimeter>::activateSensor() {
	AngularVelocimeter *velocimeter = dynamic_cast<AngularVelocimeter*>(this);
	Vector3D &v = velocimeter->getVelocity();
	VAngularVelocimeterOut *vAngularVelocimeter = new VAngularVelocimeterOut_i(v.x, v.y, v.z);
	return vAngularVelocimeter;
}

template<>
VSensorOut* CommunicableSensor<LinearVelocimeter>::activateSensor() {
	LinearVelocimeter *velocimeter = dynamic_cast<LinearVelocimeter*>(this);
	Vector3D &v = velocimeter->getVelocity();
	VLinearVelocimeterOut *vLinearVelocimeter = new VLinearVelocimeterOut_i(v.x, v.y, v.z);
	return vLinearVelocimeter;
}

template<>
VSensorOut* CommunicableSensor<PositionSensor>::activateSensor() {
	PositionSensor *positionSensor = dynamic_cast<PositionSensor*>(this);
	Vector3D pos = positionSensor->getParentEntity()->getPosition();
	VPositionSensorOut *vPositionSensor = new VPositionSensorOut_i(pos.x, pos.y, pos.z);
	return vPositionSensor;
}
